/********** Library Includes **************************************************/

#include <opencv/cv.h>
#include <opencv/highgui.h>

/********** Local Includes ****************************************************/

#include "runner.h"

#include "imagesource.h"
#include "pausemanager.h"
#include "goalstrategy.h"
#include "objectdetector.h"

#include "approxdecomp.h"
#include "common.h"
#include "celldecompastar.h"
#include "simpledrivecontroller.h"
#include "rovio.h"
#include "astarheuristics.h"

/********** Macros and Constants **********************************************/

#define DEBUG true
#define ROVIO_HOST "192.168.10.18"
#define TIME 500

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Public Methods ****************************************************/

Runner::Runner(ImageSource *i, PauseManager *p, GoalStrategy *g) :
	m_imageSource(i),
	m_pauseManager(p),
	m_goalStrategy(g)
{

}

// This all needs cleaned up

Point robot(231, 241), fruit(662, 241);

/**
 * A static helper function that returns the time with millisecond precision.
 */
long getTime();

void on_mouse(int event, int x, int y, int flags, void *)
{
	static int count = 0;
	
	switch(event) {
		case CV_EVENT_LBUTTONDOWN:
			if (count == 0) {
				robot = Point(x, y);
				cout << "Got robot position: (" << x << ", " << y << ")" << endl;
			} else if (count == 1) {
				fruit = Point(x, y);
				cout << "Got fruit position: (" << x << ", " << y << ")" << endl;
			}
			
			count++;
			break;
	}
}

int manhatten(Point a, Point b) {
	return abs(a.x - b.x) + abs(a.y - b.y);
}

void showPlan(const Mat &image, const vector<Point> plan, int blockSize, int next = -1) {
	Mat temp = image.clone();
	int count = 0;
	
	int total = 0;
	
	for (vector<Point>::const_iterator it = plan.begin(); it != plan.end(); ++it) {
		Point p(it->x * blockSize + blockSize / 2,
			it->y * blockSize + blockSize / 2);
			
		if (count == next) {
			circle(temp, p, 7, CV_RGB(0, 0, 255), CV_FILLED);
		} else {
			circle(temp, p, 5, CV_RGB(255, 0, 0), CV_FILLED);
		}
		
		count++;
		vector<Point>::const_iterator next = it + 1;
		
		if (next != plan.end()) {
			Point a(it->x * blockSize + blockSize / 2,
				it->y * blockSize + blockSize / 2);
			Point b(next->x * blockSize + blockSize / 2,
				next->y * blockSize + blockSize / 2);
				
			line(temp, a, b, CV_RGB(255, 0, 0));
			total += manhatten(a, b);
		}
	}
	
	cout << "Total path length: " << total << endl;
	
	imshow("Plan", temp);
	cvMoveWindow("Plan", 0, 900 - temp.size().height);
}

int Runner::run() {
	long startTime; 
	cout << "Press 'n' to step forward to the next frame." << endl
		<< "Press 'q' to close." << endl
		<< "In debug view:" << endl
		<< "\tRed is obstacle" << endl
		<< "\tGreen is fruit" << endl
		<< "\tBlue is robot" << endl;
	
	ObjectDetector detector(m_imageSource);
	detector.saveBackground();
	
	imshow("Processed", detector.processed());
	imshow("Raw", m_imageSource->last());
	
	cvMoveWindow("Processed", 0, 0);
	cvMoveWindow("Raw", m_imageSource->last().size().width, 0);
	
	cout << "Place obstacles and press 'n'" << endl;
	
	m_pauseManager->waitForInput();
	detector.saveObstacles();
	
	imshow("Processed", detector.processed());
	imshow("Raw", m_imageSource->last());
	
	cout << "Place and robot and fruit and press 'n'" << endl;
	
	m_pauseManager->waitForInput();

	imshow("Live", m_imageSource->next());
	cvSetMouseCallback("Live", on_mouse, 0);
	
	cout << "Click near the robot and fruit (in that order), then press 'c'"
		<< endl;
	
	int key;
	
	while (true) {
		key = waitKey(300);
		
		if (key == 'q') {
			return 0;
		} else if (key == 'c') {
			startTime = getTime();
			break;
		}
	}
	
	cvDestroyWindow("Live");
	
	if (DEBUG) {
		cvNamedWindow("Robot Blobs");
		cvMoveWindow("Robot Blobs", m_imageSource->last().size().width,
			900 - m_imageSource->last().size().height);
	}
	
	detector.saveRobotAndFruit(robot, fruit, DEBUG);
	
	Mat open = detector.getOpen();
	ApproxDecomp decomp(m_imageSource, open);
	
	Mat grid = decomp.cellDecomp();
	
	imshow("Processed", detector.processed());
	imshow("Raw", m_imageSource->last());
	
	startTime = getTime();
	CellDecompAStar astar(grid, new ManhattenDistanceHeuristic());
	
	Point robot = detector.robot().point;
	Point fruit = detector.fruit();
	
	Point start(robot.x / decomp.blockSize(), robot.y / decomp.blockSize());
	Point goal(fruit.x / decomp.blockSize(), fruit.y / decomp.blockSize());
	
	vector<Point> plan = astar.plan(start, goal);
	std::cout << "Planning time: " << getTime() - startTime << std::endl;
	showPlan(m_imageSource->last(), plan, decomp.blockSize());
	startTime = getTime();
	DUMP(plan);
	
	m_pauseManager->waitForInput();
	return 0;
}

/********** Local Functions ***************************************************/

long getTime() {
    struct timeval time;
    gettimeofday(&time, NULL);
    return ((time.tv_sec) * 1000 + time.tv_usec/1000.0) + 0.5;
}
